#include "tc.h"

__device__ float4 *d_pos;
__device__ float4 *d_rgb;
__device__ GLuint *d_look;
__device__ GLuint *d_status;
__device__ GLuint *d_weight;

__device__ GLfloat getArea(int N, GLuint *nList, float4 *pos)
{
	int i;
	GLfloat areaTimes2 = 0.0;

	for (i = 0; i < N - 1; i++) {
		areaTimes2 += ((d_pos[nList[i]].x * d_pos[nList[i + 1]].y) 
			       - (d_pos[nList[i + 1]].x * d_pos[nList[i]].y));
	}

	areaTimes2 += ((d_pos[nList[N - 1]].x * d_pos[nList[0]].y) 
		       - (d_pos[nList[0]].x * d_pos[nList[N - 1]].y));

#if defined(__TOC_DEBUG__)
	printf("DEBUG: Area(%3.3f)\n", areaTimes2 / 2);
#endif /* __TOC_DEBUG__ */

	return (areaTimes2 / 2);
}

__device__ float2 getCentroid(int N, GLuint *nList, float4 *pos)
{
	int i;
	GLfloat aggX = 0.0;
	GLfloat aggY = 0.0;
	float2 c;

	if (N == 1) {
		c.x = d_pos[nList[0]].x;
		c.y = d_pos[nList[0]].y;
	}

	GLfloat A = getArea(N, nList, d_pos);
	/* assert(A != 0); */

	if (A == 0.0)
		A = 1.0;

	c.x = 0.0;
	c.y = 0.0;

	for (i = 0; i < N - 1; i++) {
		const GLfloat commonTerm  = (d_pos[nList[i]].x * d_pos[nList[i + 1]].y) 
			- (d_pos[nList[i + 1]].x * d_pos[nList[i]].y);
		aggX += (d_pos[nList[i]].x + d_pos[nList[i + 1]].x) * commonTerm;
		aggY += (d_pos[nList[i]].y + d_pos[nList[i + 1]].y) * commonTerm;
	}

	const GLfloat commonTerm  = (d_pos[nList[N - 1]].x * d_pos[nList[0]].y) 
		- (d_pos[nList[0]].x * d_pos[nList[N - 1]].y);
	aggX += (d_pos[nList[N - 1]].x + d_pos[nList[0]].x) * commonTerm;
	aggY += (d_pos[nList[N - 1]].y + d_pos[nList[0]].y) * commonTerm;

	c.x = aggX / (6 * A);
	c.y = aggY / (6 * A);

	return c;
}

__device__ void moveToward(GLuint idx, float4 *pos, float2 c) 
{
	const GLfloat dX = d_pos[idx].x - c.x;
	/* const GLfloat dY = d_pos[idx].y - c.y; */
	GLfloat effectiveSpeed = BOID_SPEED;

	if(d_status[idx] == BOID_GHOUL) {
		d_weight[idx] += GHOUL_ACCELERATION;
		effectiveSpeed = GHOUL_SPEED * d_weight[idx];
	}

	if(dX > SPACE) {
		d_pos[idx].x -= effectiveSpeed;
		d_look[idx] = WEST;
	} else if(dX < -SPACE) {
		d_pos[idx].x += effectiveSpeed;
		d_look[idx] = EAST;
	} 

	/* if(dY > SPACE) { */
	/* 	d_pos[idx].y -= effectiveSpeed; */
	/* 	d_look[idx] = SOUTH; */
	/* } else if(dY < -SPACE) { */
	/* 	d_pos[idx].y += effectiveSpeed;		 */
	/* 	d_look[idx] = NORTH; */
	/* } */
}

/* __device__ void setDirection(boid_t *me) */
/* { */
/* 	int i; */
/* 	const GLint randDir = 0; //random() % 4; */
/* 	me->dirSync = SYNC; */
/* 	me->look = randDir; */

/* 	for(i = 0; i < me->neighborCount; i++) { */
/* 		// boid[me->neighbor[i]].dirSync = SYNC; */
/* 		// boid[me->neighbor[i]].look = randDir; */
/* 	} */
/* } */

__device__ inline GLfloat getDistance(GLuint a, GLuint b, 
				      float4 *pos)
{
	const GLfloat deltaX = pos[a].x - pos[b].x;
	const GLfloat deltaY = pos[a].y - pos[b].y;

	return (GLfloat)sqrt((deltaX * deltaX) + (deltaY * deltaY));
}


/* __device__ void findNeighbors(const GLuint idx, const GLuint status,  */
/* 			      float4 *pos,  */
/* 			      GLuint *nList, GLuint *nCount) */
/* { */
/* 	int i; */
/* 	GLfloat min = 1.0; */

/* 	if(status == BOID_GHOUL) */
/* 		return; */

/* 	for(i = 0; i < BOID_N_BODIES; i++) { */
/* 		const GLfloat dist = getDistance(idx, i, pos); */

/* 		if(dist < BOID_PROXIMITY_LIMIT && dist > 0){ */
/* 			nList[*nCount] = i; */
/* 			if(dist < min) { */
/* 				/\* me->nnDist = dist; *\/ */
/* 				/\* me->nnID = i; *\/ */
/* 			} */

/* 			*nList++; */
/* 		}		 */
/* 	} */
/* } */

__device__ void ifDangerChangeDirection(const GLuint idx, const GLuint *status, 
			      float4 *pos)
{
	int i;

	if(status[idx] == BOID_GHOUL)
		return;

	for(i = 0; i < BOID_N_BODIES; i++) {
		const GLfloat dist = getDistance(idx, i, pos);

		if(dist < BOID_PROXIMITY_LIMIT && status[i] == BOID_GHOUL){
			pos[idx].x *= -1;
			pos[idx].y *= -1;
		}		
	}
}

__device__ void scanForFood(GLuint idx, float4 *pos, 
			    GLuint *status, GLuint *nList, 
			    GLuint *nCount, GLint *nnID) 
{
	int i;
	GLfloat min = 2.0;
	*nnID = -1;

	for(i = 0; i < BOID_N_BODIES; i++) {
		if (i == idx) 
			continue;

		const GLfloat d = getDistance(idx, i, d_pos);
		if(d <= GHOUL_CANDIDATE_LIMIT && 
		   status[i] == BOID_HUMAN) {
			nList[*nCount] = i;
			*nCount++;

			if(d < min) {
				min = d;
				*nnID = i;
			}
		}
	}
}

__device__ void turnToGhoul(GLuint vIdx)
{
	d_rgb[vIdx].x = 1.0;
	d_rgb[vIdx].y = 0.0;
	d_rgb[vIdx].z = 0.0;

	d_status[vIdx] = BOID_GHOUL;
}

__device__ void moveForward(GLuint idx)
{
	const GLfloat effectiveSpeed = BOID_SPEED * d_weight[idx];

	switch(d_look[idx]) {
	case NORTH:
		d_pos[idx].y += effectiveSpeed;
		break;

	case SOUTH:
		d_pos[idx].y -= effectiveSpeed;
		break;

	case EAST:
		d_pos[idx].x += effectiveSpeed;
		break;

	case WEST:
		d_pos[idx].x -= effectiveSpeed;
		break;
	}
}

__device__ void computeGhoulActivity(GLuint idx, float4 *pos,
				     GLuint *status, GLuint *nList, 
				     GLuint *nCount) 
{
	GLuint nl[BOID_N_BODIES];
	float2 c;
	GLint nnID = -1;

	moveForward(idx);
	/* scanForFood(idx, pos, status, nl, nCount, &nnID); */

	/* if (*nCount > 0) { */
	/* 	if(nnID == -1) */
	/* 		return; */

	/* 	c = getCentroid(*nCount, nl, pos); */
	/* 	/\* moveToward(idx, pos, c); *\/ */

	/* 	if(status[nnID] == BOID_HUMAN) */
	/* 		turnToGhoul(nnID); */
	/* } */
}

__device__ void computeHumanActivity(unsigned int idx)
{
	ifDangerChangeDirection(idx, d_status, d_pos);
	moveForward(idx);
}

__global__ void computeBoid_kernel_device(float4 *pos, float4 *rgb,
					  GLuint *look, GLuint *weight,
					  GLuint *status, GLuint *nList,
					  GLuint *nCount)
{
	unsigned int myIdx = blockIdx.x * blockDim.x + threadIdx.x;

	d_pos = pos;
	d_status = status;
	d_rgb = rgb;
	d_look = look;
	d_weight = weight;

	if (status[myIdx] == BOID_HUMAN)
		computeHumanActivity(myIdx);
	else { //if(status[myIdx] == BOID_GHOUL) {
		/* moveForward(myIdx); */
		computeGhoulActivity(myIdx, pos, status, nList, nCount);
	}
}

extern "C" void computeBoid_kernel(float4 *pos, float4 *rgb,
				   GLuint *look, GLuint *weight,
				   GLuint *status, GLuint *nList,
				   GLuint *nCount)
{
	dim3 blockDim(512, 1, 1);
	dim3 gridDim(BOID_N_BODIES / blockDim.x, 1, 1);
	computeBoid_kernel_device<<<gridDim, blockDim>>>(pos, rgb,
							 look, weight,
							 status, nList,
							 nCount);
}
